/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.grt192.controller.breakaway.auto;

import com.grt192.core.StepController;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;


/**
 *
 * @author grtstudent
 */
public class BreakAwayRobotPositioningController extends StepController { //THIS CLASS IS ONLY FOR SALVAGING CODE
    //where information about the robot (position, angle, bump etc) is upadated.
    private double posX = 0.0;
    private double posY = 0.0;
    public static final double ZONE1STARTX = 7.0; //pre-determined (these are estimates)
    public static final double ZONE1STARTY = 0.0;
    public static final double ZONE2STARTX = 7.0;
    public static final double ZONE2STARTY = 16.0;
    public static final double ZONE3STARTX = 7.0;
    public static final double ZONE3STARTY = 53.0;
    public static int zone = 1; //starting zone
    private static final double DT = SLEEP_INTERVAL / 1000.0; //sec
    private double warmUpSum = 0.0;
    private double count = 0.0;
    public final double TOTAL_WARM_UP_COUNT = 200.0;
    private double warmUpAverage = 0.0;
    private double prevAngle;
    private double prevLeftDist = 0.0;
    private double prevRightDist = 0.0;

    public BreakAwayRobotPositioningController(GRTBreakawayRobotBase rb) {
        super();
        addMechanism("RobotBase", rb);

        if (zone == 1) {
            posX = ZONE1STARTX;
            posY = ZONE1STARTY;
        }
        if (zone == 2) {
            posX = ZONE2STARTX;
            posY = ZONE2STARTY;
        }
        if (zone == 3) {
            posX = ZONE3STARTX;
            posY = ZONE3STARTY;
        }

    }
    public void act() {
        GRTBreakawayRobotBase rb = (GRTBreakawayRobotBase) getMechanism("RobotBase");
        double angle = rb.getAngle();
        double leftDist = rb.getLeftDist();
        double rightDist = rb.getRightDist();

        count++;
        if (count <= TOTAL_WARM_UP_COUNT) {
            warmUpSum += angle - prevAngle;
            prevAngle = angle;
            System.out.println("Calibrating: "
                    + (count / TOTAL_WARM_UP_COUNT * 100.0)
                    + "% done");
            if(count == TOTAL_WARM_UP_COUNT) {
                warmUpAverage = warmUpSum / TOTAL_WARM_UP_COUNT;
            }
        } else {
            angle -= warmUpAverage * count; //offset not constant
            System.out.println("angle: " + angle);
        }

        updatePos(leftDist, rightDist, angle);
        prevLeftDist = leftDist;
        prevRightDist = rightDist;
        System.out.println("x: " + posX);
        System.out.println("y " + posY);
    }

    public void updatePos(double leftDist, double rightDist, double angle) //angle in degrees, math style
    {

        posX += Math.cos(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
                (rightDist - prevRightDist)) / 2.0;
        posY += Math.sin(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
                (rightDist - prevRightDist)) / 2.0;
//        GRTBreakawayRobotBase rb = (GRTBreakawayRobotBase) getMechanism("RobotBase");
//        rb.updatePos(posX, posX, angle);
    }
}
